#include "blobtracker.h"

BlobTracker::BlobTracker()
{

}

BlobTracker::~BlobTracker()
{

}

void BlobTracker::predictNextPosition(TargetBlob &existingBlob)
{
  int numPositions = existingBlob.centerPositions.size();

  if (numPositions == 1) {
    existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x;
    existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y;
  }
  else if (numPositions == 2) {
    int deltaX = existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x;
    int deltaY = existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y;
    existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
    existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
  }
  else if (numPositions == 3) {
    int sumOfXChanges = ((existingBlob.centerPositions[2].x - existingBlob.centerPositions[1].x) * 2) + ((existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x) * 1);
    int deltaX = (int)std::round((float)sumOfXChanges / 3.0);
    int sumOfYChanges = ((existingBlob.centerPositions[2].y - existingBlob.centerPositions[1].y) * 2) + ((existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y) * 1);
    int deltaY = (int)std::round((float)sumOfYChanges / 3.0);
    existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
    existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
  }
  else if (numPositions == 4) {
    int sumOfXChanges = ((existingBlob.centerPositions[3].x - existingBlob.centerPositions[2].x) * 3) + ((existingBlob.centerPositions[2].x - existingBlob.centerPositions[1].x) * 2) + ((existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x) * 1);
    int deltaX = (int)std::round((float)sumOfXChanges / 6.0);
    int sumOfYChanges = ((existingBlob.centerPositions[3].y - existingBlob.centerPositions[2].y) * 3) + ((existingBlob.centerPositions[2].y - existingBlob.centerPositions[1].y) * 2) + ((existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y) * 1);
    int deltaY = (int)std::round((float)sumOfYChanges / 6.0);
    existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
    existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
  }
  else if (numPositions >= 5) {
    int sumOfXChanges = ((existingBlob.centerPositions[numPositions - 1].x - existingBlob.centerPositions[numPositions - 2].x) * 4) + ((existingBlob.centerPositions[numPositions - 2].x - existingBlob.centerPositions[numPositions - 3].x) * 3) + ((existingBlob.centerPositions[numPositions - 3].x - existingBlob.centerPositions[numPositions - 4].x) * 2) + ((existingBlob.centerPositions[numPositions - 4].x - existingBlob.centerPositions[numPositions - 5].x) * 1);
    int deltaX = (int)std::round((float)sumOfXChanges / 10.0);
    int sumOfYChanges = ((existingBlob.centerPositions[numPositions - 1].y - existingBlob.centerPositions[numPositions - 2].y) * 4) + ((existingBlob.centerPositions[numPositions - 2].y - existingBlob.centerPositions[numPositions - 3].y) * 3) + ((existingBlob.centerPositions[numPositions - 3].y - existingBlob.centerPositions[numPositions - 4].y) * 2) + ((existingBlob.centerPositions[numPositions - 4].y - existingBlob.centerPositions[numPositions - 5].y) * 1);
    int deltaY = (int)std::round((float)sumOfYChanges / 10.0);
    existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
    existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
  }
  else {
    //should never get here!
  }
}

void BlobTracker::matchCurrentFrameBlobsToExistingBlobs(std::vector<TargetBlob> &existingBlobs, std::vector<TargetBlob> &currentFrameBlobs)
{
  for (auto &existingBlob : existingBlobs) {
    existingBlob.currentMatchFoundOrNewBlob = false;
    predictNextPosition(existingBlob);
  }

  for (auto &currentFrameBlob : currentFrameBlobs) {

    int intIndexOfLeastDistance = 0;
    double leastDistance = 1000000.0;

    for (unsigned int i = 0; i < existingBlobs.size(); i++) {
      if (existingBlobs[i].stillBeingTracked == true) {
        double dblDistance = distanceBetweenPoints(currentFrameBlob.centerPositions.back(), existingBlobs[i].predictedNextPosition);
        if (dblDistance < leastDistance) {
          leastDistance = dblDistance;
          intIndexOfLeastDistance = i;
        }
      }
    }

    if (leastDistance < currentFrameBlob.currentDiagonalLength * 0.5) {
      addBlobToExistingBlobs(currentFrameBlob, existingBlobs, intIndexOfLeastDistance);
    }
    else {
      addNewBlob(currentFrameBlob, existingBlobs);
    }
  }

  for (unsigned int i = 0; i < existingBlobs.size(); i++) {
    if (existingBlobs[i].currentMatchFoundOrNewBlob == false) {
      //miss number
      existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch++;
    }
    if (existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch >= 5) {
      //giveup tracking
      existingBlobs[i].stillBeingTracked = false;
    }

    //if (existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch >= 50) {
    //  existingBlobs.erase(existingBlobs.begin() + i);
    //  i--;
    //}
  }
}

void BlobTracker::addBlobToExistingBlobs(TargetBlob &currentFrameBlob, std::vector<TargetBlob> &existingBlobs, int &intIndex)
{
  existingBlobs[intIndex].currentContour = currentFrameBlob.currentContour;
  existingBlobs[intIndex].currentBoundingRect = currentFrameBlob.currentBoundingRect;
  existingBlobs[intIndex].currentDiagonalLength = currentFrameBlob.currentDiagonalLength;
  existingBlobs[intIndex].currentAspectRatio = currentFrameBlob.currentAspectRatio;
  existingBlobs[intIndex].stillBeingTracked = true;
  existingBlobs[intIndex].currentMatchFoundOrNewBlob = true;

  existingBlobs[intIndex].rois.push_back(currentFrameBlob.rois.back());
  existingBlobs[intIndex].postrois.push_back(currentFrameBlob.postrois.back());

  existingBlobs[intIndex].centerPositions.push_back(currentFrameBlob.centerPositions.back());
}

void BlobTracker::addNewBlob(TargetBlob &currentFrameBlob, std::vector<TargetBlob> &existingBlobs)
{
  currentFrameBlob.currentMatchFoundOrNewBlob = true;
  existingBlobs.push_back(currentFrameBlob);
}

double BlobTracker::distanceBetweenPoints(cv::Point point1, cv::Point point2)
{
  int intX = abs(point1.x - point2.x);
  int intY = abs(point1.y - point2.y);
  return(sqrt(pow(intX, 2) + pow(intY, 2)));
}

